#include "update_by_pos.hpp"

namespace sros::location
{
    void update_by_pos::update()
    {
        std::lock_guard<std::mutex> lck(mtx_);
        auto predict_ = param_->P_;

        if (!predict_)
        {
            return;
        }
        
        auto predict_param = predict_->param();

        // update 1
        Eigen::VectorXd y(1);
        y << param_->Z_ - param_->H_ * predict_param.X_;
        Eigen::Matrix<double, 3, 1> K = predict_param.P_ * param_->H_.transpose() * (1.0/(param_->H_*predict_param.P_*param_->H_.transpose() + param_->R_));
        
        // update 2
        predict_param.X_ = predict_param.X_ + K + y;
        predict_->set_X(predict_param.X_);

        // update 3
        predict_param.P_ = (Eigen::Matrix3d::Identity() - K*param_->H_) * predict_param.P_;
        predict_->set_P(predict_param.P_);
    }

    void update_by_pos::init_()
    {
        // 观测矩阵
        param_->H_ << 1, 0, 0;

        // 观测噪声
        param_->R_ = 0.1;

        // 观测值
        param_->Z_ = 0.0;
    }
}